#ifndef _SPARSE_ARRAY_H
#define _SPARSE_ARRAY_H

#include <map>
#include <vector>
#include <assert.h>
#include <iostream>
#include <math.h>

#include <boost/shared_ptr.hpp>

//template <class Type>
class SparseArray
{
    friend std::ostream& operator<<(std::ostream&, const SparseArray&);
public:
    SparseArray(int length) : 
        _length(length), _active(0) {}

    ~SparseArray() {}

    // assert an item into the array
    void insert(const int &i, float v)
    {
        assert(i >= 0);
        assert(i < _length);
        if(v != 0)
        {
            _index.push_back(i);    
            _values.push_back(v);    
            _active++;
        }
    }

    // vector operations
    float dot(const SparseArray *rhs) const
    {
        // ASSUMES SORTED INDEXES!
        float sum = 0;
        int lhs_index=0, rhs_index=0;

        // iterate over the lhs's indexes
        for(; lhs_index < _active; ++lhs_index)
        {
            // shift the rhs index forward 
            while(rhs_index < rhs->active() && rhs->_index[rhs_index] < _index[lhs_index])
                  rhs_index++;

            // if we runout of rhs indexes, stop
            if(rhs_index >= rhs->active()) break;
            
            // if the indexes match, update dot product
            if(rhs->_index[rhs_index] == _index[lhs_index])
                sum += _values[lhs_index]*rhs->_values[rhs_index];
        }
        return sum;
    }

    float l2_difference_norm_sqr(const SparseArray *rhs) const
    {
        // ASSUMES SORTED INDEXES!
        float sum = 0;

        // iterate over the lhs's indexes
        int lhs_index=0, rhs_index=0;
        for(; lhs_index < _active; ++lhs_index)
        {
            // shift the rhs index forward 
            while(rhs_index < rhs->active() && rhs->_index[rhs_index] < _index[lhs_index])
                  rhs_index++;

            // if the indexes match, update dot product
            if(rhs_index < rhs->active() && rhs->_index[rhs_index] == _index[lhs_index])
            {
                sum += pow((_values[lhs_index]-rhs->_values[rhs_index]),2);
            }
            else
            {
                sum += pow(_values[lhs_index],2);
            }
        }
        // iterate over the rhs's indexes
        lhs_index=0; rhs_index=0;
        for(; rhs_index < rhs->active(); ++rhs_index)
        {
            // shift the rhs index forward 
            while(lhs_index < _active && _index[lhs_index] < rhs->_index[rhs_index])
                  lhs_index++;

            // if the indexes match, update dot product
            if(lhs_index < _active && _index[lhs_index] != rhs->_index[rhs_index])
            {
                sum += pow(-rhs->_values[rhs_index],2);
            }
        }
        return sum;
    }

	SparseArray operator+(const SparseArray &rhs)
	{
		assert(_length==rhs._length);
        // ASSUMES SORTED INDEXES!
        int lhs_index=0, rhs_index=0;
		SparseArray result(_length);;

        // iterate over the lhs's indexes
        for(; lhs_index < _active; ++lhs_index)
        {
            // shift the rhs index forward 
            while(rhs_index < rhs.active() && rhs._index[rhs_index] < _index[lhs_index])
			{
                  result.insert(rhs._index[rhs_index], rhs._values[rhs_index]);
                  rhs_index++;
			}

            // if we runout of rhs indexes, stop
//            if(rhs_index >= rhs.active()) break;
            
            // if the indexes match, insert their sum 
            if(rhs_index < rhs.active() && rhs._index[rhs_index] == _index[lhs_index])
			{
                result.insert(_index[lhs_index], _values[lhs_index]+rhs._values[rhs_index]);
				rhs_index++;
			}
			else
                result.insert(_index[lhs_index], _values[lhs_index]);
        }

        // iterate over the rhs's indexes which haven't been added
        for(; rhs_index < rhs.active(); ++rhs_index)
			result.insert(rhs._index[rhs_index], rhs._values[rhs_index]);

		return result;
	}

    int length( void ) const { return _length; }
    int active( void ) const { return _active; }
    void set_length(int l) { _length = l; }

public:
    std::vector<int> _index;
    std::vector<float> _values;

private:
    int _length;
    int _active;
};
typedef boost::shared_ptr<SparseArray> SparseArrayPtr;

inline std::ostream& operator<<(std::ostream &os, const SparseArray &sa)
{
    os << "<";
    for(int i=0; i < sa._active; ++i)
        os << '(' << sa._index[i] << ',' << sa._values[i] << ')';
    os << ">";

    return os;
}

class SparseMatrix2d
{
    friend std::ostream& operator<<(std::ostream&, const SparseMatrix2d&);

//    struct _Index { int row, column; };
//    typedef std::pair<int,int> Index;
    typedef int Index;

public:
    SparseMatrix2d(int rows, int columns) : 
        _rows(rows), _columns(columns) {}

    SparseMatrix2d(int rows, int columns, std::map<Index,double> &assignments) 
        : _rows(rows), _columns(columns), _assignments(assignments) {}

    ~SparseMatrix2d() {}

    // retrieve an item value
    double operator()(int row, int column)  const 
    {
        assert(row >= 0 && row < _rows);
        assert(column >= 0 && column < _columns);

//        if(_assignments.count(std::pair<int,int>(row,column)) == 0)
//        _indexCache.first = row; _indexCache.second = column;
        _indexCache = row*_rows + column;
//        if(_assignments.count(_indexCache) == 0)
//            return 0;
        return _assignments.find(_indexCache)->second;
    }

    // assignment
    double& operator()(int row, int column)
    {
        assert(row >= 0 && row < _rows);
        assert(column >= 0 && column < _columns);

//        std::pair<int,int> i = std::pair<int,int>(row,column);
//        _indexCache.first = row; _indexCache.second = column;
        _indexCache = row*_rows + column;

        if(_assignments.count(_indexCache) == 0)
            _assignments[_indexCache] = 0;

        return _assignments.find(_indexCache)->second;
    }

    void set(int row, int column, double val)
    {
        assert(row >= 0 && row < _rows);
        assert(column >= 0 && column < _columns);
        
//        _indexCache.first = row; _indexCache.second = column;
        _indexCache = row*_rows + column;
        if (val != 0)
            _assignments[_indexCache] = val;
        else
            _assignments.erase(_indexCache);

        return;
    }

    // access this arrays map
    std::map<Index, double> assignments(void) 
        { return _assignments; }

    int rows( void ) const
    {
        return _rows;
    }

    int columns( void ) const
    {
        return _columns;
    }

private:
    mutable Index _indexCache;
    int _rows, _columns;
    std::map<Index,double> _assignments;

};

inline std::ostream& operator<<(std::ostream &os, const SparseMatrix2d &sm)
{
    os << "<";
    std::map<SparseMatrix2d::Index,double>::const_iterator it;
    for(it=sm._assignments.begin(); it != sm._assignments.end(); ++it)
//        os << "[(" << it->first.first << ',' << it->first.second << ") " << it->second << ']';
        os << "[(" << it->first / sm.rows() << ',' << it->first % sm.rows() << ") " << it->second << ']';
    os << ">";

    return os;
}

#endif // _SPARSE_ARRAY_H
